package novemberBot.controller;
import novemberBot.FactoryFloorRobot;
import novemberBot.mapmaking.PathFinder;
import novemberBot.utils.Debugger;
import novemberBot.utils.LCDInfo;
import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.LCD;
import lejos.nxt.NXT;


/**Main class for master brick
 * @author Antoine Lennartz*/
public class MasterMain {
	/**link to robot object*/
	public static FactoryFloorRobot ffr=new FactoryFloorRobot(true);
	/**link to LCDInfo object*/
	public static LCDInfo lcdi;
	/**link to ultrasonic localiser*/
	public static Localizer loc;
	/**PathFinder*/
	public static PathFinder theOvermind;
	/**boolean for enabling/disabling the use of light sensors (for testing)*/
	public static boolean light = true;
	
	 
	/**main method for master brick executes main program flow*/
	public static void main(String[] args) {
		initialize();

		light=true;
		localize();
		
		ffr.startMapMaking();
		ffr.getNavigator().useSnap(light);
		ffr.getNavigator().setPath(theOvermind.makePathTo(FactoryFloorRobot.DROP_OFF), false);
		
		
		try {
			Thread.sleep(60000);
		} catch (InterruptedException e) {}
	}
	

	/**mothod used to test RS485 transmission delay*/
	public static void testPing(int tests, int length){
		ffr.getUsPoller().pauseSweep();
		long[] pings = new long[tests];
		for(int i=0; i<tests; i++){
			pings[i]=ffr.getConnectionManager().ping(length);
		}
		double mean=0;
		for(int i=0; i<tests; i++){
			mean+=pings[i];
		}
		mean/=tests;
		double stdDev=0;
		for(int i=0; i<tests; i++){
			stdDev+=(pings[i]-mean)*(pings[i]-mean);
		}
		stdDev=Math.sqrt(stdDev/tests);
		LCD.clear();
		System.out.println("tests: "+tests+"\nlength: "+length+"\nmean: "+mean+"\nstdDev: "+stdDev);
	}
	
	public static void localize(){
		ffr.getArmRemote().openClawTo(165, false);
		loc.usLocalize();
		ffr.getNavigator().setTarget(new double[]{30.48, 30.48, 0}, false);
		if(light){
			loc.lightLocalize();
		ffr.getNavigator().setTarget(new double[]{30.48, 30.48, 0}, false);
		}
		
		ffr.getArmRemote().openClawTo(0, false);
		
	}
	
	
	
	
	/**initialises class objects and calls ffr.initialize()*/
	public static void initialize(){
		ffr.initialize();
		lcdi= new LCDInfo();
		loc = new Localizer();
		ffr.getUsPoller().addResponder(loc);
		Debugger.setLCDInfo(lcdi);
		theOvermind = new PathFinder();
		ffr.getNavigator().setPathFinder(theOvermind);
		
		Button.ESCAPE.addButtonListener(new ButtonListener() {
				
				@Override
				public void buttonPressed (Button b){
					NXT.exit(0);
				}
				@Override
				public void buttonReleased (Button b){
					NXT.exit(0);
				}
				
			});
	}
	
}
